﻿#include "dxl360comm.h"

DXL360Comm::DXL360Comm()
    :m_state(0),
      m_x(NAN),
      m_y(NAN)
{
    m_bufferX[5] = 0;
    m_bufferY[5] = 0;
    connect(&m_port, SIGNAL(readyRead()), this, SLOT(on_readyRead()));
}

void DXL360Comm::stop()
{
    m_port.close();
}

void DXL360Comm::setPortName(QString name)
{
    m_port.setPortName(name);
}

bool DXL360Comm::value(double &x, double &y)
{
    x = m_x;
    y = m_y;
    QDateTime t = QDateTime::currentDateTime();
    return (t.msecsTo(m_time) > -1000);
}


DXL360Comm::~DXL360Comm()
{
    m_port.close();
    disconnect(&m_port, SIGNAL(readyRead()), this, SLOT(on_readyRead()));
}

bool DXL360Comm::start()
{
    m_port.setBaudRate(QSerialPort::Baud9600);
    return m_port.open(QIODevice::ReadOnly);

}

void DXL360Comm::stateMachine(char x)
{
    switch (m_state)
    {
    case 0:
        if(x == 'X')
        {
            m_state = 'X';
            m_count = 0;
        }
        break;
    case 'X':
        if(x == 'Y')
        {
            m_state = 'Y';
            m_count = 0;
        }
        else if(m_count >= 5) //
        {
            m_state = 0; //到这里说明有问题了。
            m_count = 0;
        }
        else
        {
            m_bufferX[m_count] = x;
            m_count++;
        }
        break;
    case 'Y':
        m_bufferY[m_count] = x;
        m_count++;
        if(m_count == 5)
        {
            m_state = 0;
            m_x = atoi(m_bufferX) / 100.0;
            m_y = atoi(m_bufferY) / 100.0;
            m_time = QDateTime::currentDateTime();
            emit dataReady(m_x, m_y);
        }
        break;
    default:
        m_state = 0;
        break;
    }
}

void DXL360Comm::on_readyRead()
{
    QByteArray data = m_port.readAll();
    QByteArray::const_iterator it;
    for (it = data.constBegin(); it != data.constEnd(); ++it)
    {
          stateMachine(*it);
    }
}
